import numpy
import math
from numpy      import linalg as LA
from obstacle   import *
from math import sqrt
import helper

class Cube(Obstacle):
    """The class representing a cube."""
    
    state           = 0;
    speed           = 0;
    rotSpeed        = 0;
    resizeSpeed     = 0;
    radius          = 0;
    sqrRadius       = 0;
    futureRadius    = 0;
    cheat           = -0.15;
    positiveInf     = 100000;
    minValue        = 0.05;
    
    middle          = numpy.zeros((3),    dtype=float);
    dir             = numpy.zeros((3),    dtype=float);
    rotationAxis    = numpy.zeros((3),    dtype=float); 
    vertices        = numpy.zeros((8,3),  dtype=float);
    walls           = numpy.zeros((6,3),  dtype=float);
    invWalls        = numpy.zeros((6,4,3), dtype=float);
    
    def __init__(self, mX, mY, mZ, size):
        self.middle       = numpy.array([mX, mY, mZ]);
        
        x = size / 2;
        
        numpy.add(numpy.array([-x,-x, x]), self.middle, self.vertices[0]);
        numpy.add(numpy.array([ x,-x, x]), self.middle, self.vertices[1]);
        numpy.add(numpy.array([ x,-x,-x]), self.middle, self.vertices[2]);
        numpy.add(numpy.array([-x,-x,-x]), self.middle, self.vertices[3]);
        numpy.add(numpy.array([-x, x, x]), self.middle, self.vertices[4]);
        numpy.add(numpy.array([ x, x, x]), self.middle, self.vertices[5]);
        numpy.add(numpy.array([ x, x,-x]), self.middle, self.vertices[6]);
        numpy.add(numpy.array([-x, x,-x]), self.middle, self.vertices[7]);
        
        numpy.subtract(self.vertices[1], self.vertices[0], self.walls[0]);
        numpy.subtract(self.vertices[3], self.vertices[0], self.walls[1]);
        numpy.subtract(self.vertices[0], self.vertices[1], self.walls[2]);
        numpy.subtract(self.vertices[1], self.vertices[2], self.walls[3]);
        numpy.subtract(self.vertices[4], self.vertices[0], self.walls[4]);
        numpy.subtract(self.vertices[0], self.vertices[4], self.walls[5]);
        
        vectNorms = numpy.array(
                                [LA.norm(self.walls[0]), 
                                 LA.norm(self.walls[1]), 
                                 LA.norm(self.walls[2]), 
                                 LA.norm(self.walls[3]), 
                                 LA.norm(self.walls[4]), 
                                 LA.norm(self.walls[5])]); 
        
        self.walls[0] = self.walls[0] / vectNorms[0];
        self.walls[1] = self.walls[1] / vectNorms[1];
        self.walls[2] = self.walls[2] / vectNorms[2];
        self.walls[3] = self.walls[3] / vectNorms[3];
        self.walls[4] = self.walls[4] / vectNorms[4];
        self.walls[5] = self.walls[5] / vectNorms[5];
        
        for i in range(6):
            self.invWalls[i] = numpy.array(
                                           [-self.walls[i],
                                            -self.walls[i],
                                            -self.walls[i],
                                            -self.walls[i]]);  
                                        
        temp = self.vertices[0] - self.middle;
        self.futureSize = LA.norm(temp);
    def CollisionHandle(self, point):
        temp = numpy.zeros(6);
        temp[0:5] = 10000;
        tempVertex = numpy.zeros((6,3),  dtype=float);
        
#        print self.cheat
        
        tempVertex[0] = self.vertices[0] + self.cheat * self.walls[1];
        tempVertex[1] = self.vertices[1] + self.cheat * self.walls[2];
        tempVertex[2] = self.vertices[2] + self.cheat * self.walls[3];
        tempVertex[3] = self.vertices[0] + self.cheat * self.walls[0];
        tempVertex[4] = self.vertices[0] + self.cheat * self.walls[4];
        tempVertex[5] = self.vertices[4] + self.cheat * self.walls[5];
        
        #print tempVertex
        
        threshold = 0.0;
        
        temp[1] = numpy.dot(self.walls[1], point - tempVertex[0]);
        temp[1] = self._CheckIfNan(temp[1]);
        if temp[1] < threshold:
            return point;
        temp[2] = numpy.dot(self.walls[2], point - tempVertex[1]);
        temp[2] = self._CheckIfNan(temp[2]);
        if temp[2] < threshold:
            return point;
        temp[3] = numpy.dot(self.walls[3], point - tempVertex[2]);
        temp[3] = self._CheckIfNan(temp[3]);
        if temp[3] < threshold:
            return point;
        temp[0] = numpy.dot(self.walls[0], point - tempVertex[3]);
        temp[0] = self._CheckIfNan(temp[0]);
        if temp[0] < threshold:
            return point;
        temp[4] = numpy.dot(self.walls[4], point - tempVertex[4]);
        temp[4] = self._CheckIfNan(temp[4]);
        if temp[4] < threshold:
            return point;
        temp[5] = numpy.dot(self.walls[5], point - tempVertex[5]);
        temp[5] = self._CheckIfNan(temp[5]);
        if temp[5] < threshold:
            return point;
         
        minIdx = self._MinIndex(temp, 6);
                        
        point = point - self.walls[minIdx] * self.minValue;
        
        return point;
    def _MinIndex(self, vector, size):
        min = self.positiveInf;
        minIndex = -1;
        for i in range(size):
            if min > vector[i]:
                min = vector[i];
                minIndex = i;
        return minIndex;
    def _CheckIfNan(self, value):
        if math.isnan(value):
            value = self.minValue;
        return value;    
    def StaticMove(self, dx, dy, dz):
        moveArray = numpy.array([dx, dy, dz]);
        
        #print self.vertices;
        for i in range(8):
            numpy.subtract(self.vertices[i], moveArray, self.vertices[i]);
        #print self.vertices;
    def StaticResize(self, factor):
        temp = numpy.zeros((3), dtype=float);
        numpy.add(self.vertices[0], self.vertices[2], temp);
        numpy.subtract(self.vertices[0], temp, temp);
        length = LA.norm(temp);
        self._Resize((factor - 1) * length);
    def Resize(self, factor, speed):
        temp = numpy.zeros((3), dtype=float);
        numpy.add(self.vertices[0], self.vertices[2], temp);
        numpy.subtract(self.vertices[0], temp, temp);
        length = LA.norm(temp);
        if speed != 0:
            self.resizeSpeed = speed;
            self.futureSize = factor * length;
        else:
            self.resizeSpeed = 1;
            self.futureSize = length;
    def _Resize(self, factor):
        middle = (self.vertices[0] + self.vertices[2]) / 2;
        temp   = numpy.zeros(3);
        for i in range(8):
            numpy.subtract(self.vertices[i], middle, temp);
            norm = LA.norm(temp);
            temp /= norm;
            temp *= factor;
            self.vertices[i] += temp;
    def Move(self, x, y, z, speed):
        len = sqrt(x*x + y*y + z*z);
        
        if len != 0:
            self.dir = numpy.array([x / len, y / len, z / len]);
        else:
            self.dir = numpy.zeros(3);
        
        self.speed = speed;
    def SetState(self, newState):
        self.state = newState;
    def Rotate(self, x, y, z, speed):
        self.rotationAxis = numpy.array([x, y, z]);
        self.rotSpeed = speed;
        R = helper.rotate_matrix(self.rotationAxis, self.rotSpeed)
        T = numpy.matrix(numpy.empty_like(self.vertices))
        T[:] = self.middle
        vertices = numpy.matrix(self.vertices)
        vertices[:] -= T
        vertices[:] *= numpy.transpose(R)
        vertices[:] += T
        self.vertices[:] = vertices[:]
        walls = numpy.matrix(self.walls)
        walls[:] = walls * numpy.transpose(R)
        self.walls[:] = walls[:]
        for i in range(6):
            self.invWalls[i] = numpy.array(
                                           [-self.walls[i],
                                            -self.walls[i],
                                            -self.walls[i],
                                            -self.walls[i]]);
                                            
    def StaticRotate(self):
#        Will not be implemented in the first release
        raise NotImplementedError( "Cube implementation should come here..." )
